import socket_server
import reeds_shepp_path_planning as rspp
import pure_pursuit as pur
import matplotlib.pyplot as plt
import threading
import time
import numpy as np
from pid_control import v_angle


class Car:
    class Carstate:
        def __init__(self, x = None, y = None, v = None, yaw = None, t = None):
            self.x = x
            self.y = y
            self.v = v
            self.yaw = yaw
            self.t = t
            
    def __init__(self):
        self.target_speed = 40  # m/s
        self.T = 1000  # max simulation time
        self.flag = False   #小车是否处在运行状态
        self.carstate = self.Carstate()
        plt.ion()   #将画图模式改为交互模式

    # 计算每个节点的速度
    # 输入：reeds_shepp规划后返回的x,y,yaw集合
    # 输出：每个节点到下个节点的运动状态states集合
    def pure_purs(self, x, y, yaw):
        T = self.T
        target_speed = self.target_speed
        cx = x
        cy = y
        cyaw = yaw
        state = pur.State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
        lastIndex = len(cx) - 1
        time = 0.0
        states = pur.States()
        states.append(time, state)
        target_course = pur.TargetCourse(cx, cy)
        target_ind, _ = target_course.search_target_index(state)

        while T >= time and lastIndex > target_ind:
            # Calc control input
            ai = pur.proportional_control(target_speed, state.v)
            di, target_ind = pur.pure_pursuit_steer_control(
                state, target_course, target_ind)

            state.update(ai, di)  # Control vehicle

            time += pur.dt
            states.append(time, state)
        return states

    # 规划两点之间的路径
    # 输入：基站端下发start（x, y, yaw）,end(x, y, yaw)
    # 输出：路径上每个节点的信息point（x,y,yaw），flag 路线规划是否成功
    def reeds_shepp(self, x, y):
        start_x, start_y, start_yaw = x
        start_yaw = np.deg2rad(start_yaw)
        end_x, end_y, end_yaw = y
        end_yaw = np.deg2rad(end_yaw)
        curvature = 1/40
        step_size = 0.1
        px, py, pyaw, mode, clen = rspp.reeds_shepp_path_planning(
            start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
        if px is not None:
            print("reeds shepp ok")
            self.flag = True
            return px, py, pyaw

    # 可视化计算后的数据
    # 输入： states 运动状态集合
    def vision(self, states):
        plt.clf()
        #plt.cla()
        plt.subplot(311)
        plt.plot(states.x, states.y, "-b")
        plt.legend()
        plt.xlabel("x[m]")
        plt.ylabel("y[m]")
        plt.axis("equal")
        plt.grid(True)

        plt.subplot(312)
        plt.plot(states.t, [iv * 3.6 for iv in states.v], "-r")
        plt.xlabel("Time[s]")
        plt.ylabel("Speed[km/h]")
        plt.grid(True)

        plt.subplot(313)
        plt.plot(states.t, states.yaw, "-r")
        plt.xlabel("Time[s]")
        plt.ylabel("yaw")
        plt.grid(True)
        plt.pause(0.1)

    # 一直运行，控制小车运动
    def carMotion(self):
        while True:
            #路径规划结束后，flag转为true
            if self.flag:
                ####################################################
                ##################添加动作代码#######################
                ####################################################
                for t in self.carstate.t:
                    #v_angle(self.carstate.v, self.carstate.yaw)#v是以图像像素为单位的
                    time.sleep(t)
                #v_angle(0, 0)#回归静止，前轮摆正
                #运动结束后，flag转为false
                self.flag = False
                
def main():
    socket = socket_server.socket_Server()
    car = Car()
    t1 = threading.Thread(target=socket.server_open, args=())
    t1.setDaemon(True)
    t1.start()
    # car control command always on
    t2 = threading.Thread(target=car.carMotion, args=())
    t2.start()
    while True:
        Takeflag, rec = socket.server_takeData()
        if Takeflag:
            # rec = socket.rec
            print("rec:{}".format(rec))
            # time.sleep(2)
            
            string = rec.split("=")
            if string[0] == 'start':
                start_x = string[1].split(",goal")[0]
                start_x = eval(start_x)
                end_y = string[2]
                end_y = eval(end_y)
                x, y, yaw = car.reeds_shepp(start_x, end_y)

                state = car.pure_purs(x, y, yaw)
                
                if car.flag:
                    car.carstate = car.Carstate(state.x, state.y, state.v, state.yaw, state.t)
                    car.vision(state)
                    socket.server_send("{};{};OK".format(state.x, state.y))
                    # socket.server_send("OK")
                else:
                    socket.server_send("NO")
            elif string[0] == 'Stationary':
                if car.flag: socket.server_send("N")
                else: socket.server_send("Y")
            elif string[0] == 'exit':
                print("this is exit")
                break
            else:
                print("unknown cmd, continue")
    
    socket.server_close()
    #t1.join()


if __name__ == "__main__":
    main()


